#include "attitude_control.h"
#include "PID.h"

#define PitchRateIoutLimit 80
#define PitchRateLimit 300
#define PitchLimit 2000


PIDOut pidPitchRate, pidRollRate, pidYawRate,pidPitch,pidRoll,pidYaw;
extern PIDOut pidHeightRate;
extern volatile float throttleBasic;
	
void Calculate_output(void);


void Attitude_PID_Init(void)
{
	/*inner pid*/
	pidPitchRate.iOut_limit_max=PitchRateIoutLimit;
	pidPitchRate.iOut_limit_min=-PitchRateIoutLimit;
	pidPitchRate.limit_max=PitchRateLimit;
	pidPitchRate.limit_max=-PitchRateLimit;
	
	pidRollRate.iOut_limit_max=PitchRateIoutLimit;
	pidRollRate.iOut_limit_min=-PitchRateIoutLimit;
	pidRollRate.limit_max=PitchRateLimit;
	pidRollRate.limit_max=-PitchRateLimit;
	
	pidYawRate.iOut_limit_max=PitchRateIoutLimit;
	pidYawRate.iOut_limit_min=-PitchRateIoutLimit;
	pidYawRate.limit_max=PitchRateLimit;
	pidYawRate.limit_max=-PitchRateLimit;
	
	/*outer pid*/
	pidPitch.limit_max=PitchLimit;
	pidPitch.limit_min=-PitchLimit;
	Para_Info.Pitch.Ki=0;
	
	pidRoll.limit_max=PitchLimit;
	pidRoll.limit_min=-PitchLimit;
	Para_Info.Roll.Ki=0;
	
	pidYaw.limit_max=PitchLimit;
	pidYaw.limit_min=-PitchLimit;
	Para_Info.Yaw.Ki=0;
}


/**
* @Description 姿态角速度内环pid控制
 */
void Attitude_Inner_Control(void)
{
	static unsigned int AttitudeInnertPre = 0; //上一次进行内环控制的时间戳
	
	/************************ 内环Pitch角速度环调节************************/
	//如果要测试Pitch角打舵内环实验，把期望（pidPitch.value）改为—>	Target_Info.RatePitch
	float PitchRate_Erro;

	if (FlightControl.droneMode != Drone_Mode_RatePitch)
	{
		//四轴飞行模式：误差=外环输出的pitch-实际测量的pitch角速度
		PitchRate_Erro = (pidPitch.value - RT_Info.ratePitch);
	}
	else 
	{
		//调试模式：误差 = 上位机角速度期望 - 当前角速度
		PitchRate_Erro = (Target_Info.RatePitch - RT_Info.ratePitch);
	}
	Para_Info.PitchRate_Erro.Erro=PitchRate_Erro;
	//计算pid输出
	pid_calculate(&Para_Info.PitchRate_Erro,&AttitudeInnertPre,&Para_Info.ratePitch,&pidPitchRate);
	
	/************************ 内环Roll角速度环调节************************/
	float RollRate_Erro;

	if (FlightControl.droneMode != Drone_Mode_RatePitch)
	{

		RollRate_Erro = (pidPitch.value - RT_Info.ratePitch);
	}
	else 
	{

		RollRate_Erro = (Target_Info.RatePitch - RT_Info.ratePitch);
	}
	Para_Info.RollRate_Erro.Erro=RollRate_Erro;
	pid_calculate(&Para_Info.RollRate_Erro,&AttitudeInnertPre,&Para_Info.rateRoll,&pidRollRate);	
	
	/************************ 内环Yaw角速度环调节************************/
	//遥控器直接控制yaw的角速度
	float YawRate_Erro = (7.5f * Control_Info.yV - RT_Info.rateYaw);	
	
	Para_Info.YawRate_Erro.Erro=RollRate_Erro;
	pid_calculate(&Para_Info.YawRate_Erro,&AttitudeInnertPre,&Para_Info.rateYaw,&pidYawRate);	

	//输出到电机
	Calculate_output();
}

/**
* @Description 姿态角度外环pid控制
 */
void Attitude_Outer_Control(void)
{
	static unsigned int AttitudeOutertPre = 0; //上一次进行内环控制的时间戳
	
	/* Pitch 角度控制：期望角度 - 修正后的实际角度 */
	float Pitch_Erro=(Target_Info.Pitch-(RT_Info.Pitch-Errangle_Info.fixedErroPitch));
	
	Para_Info.Pitch_Erro.Erro=Pitch_Erro;
	pid_calculate(&Para_Info.Pitch_Erro,&AttitudeOutertPre,&Para_Info.Pitch,&pidPitch);	
	
	/* Roll 角度控制 */
	float Roll_Erro=(Target_Info.Roll-(RT_Info.Roll-Errangle_Info.fixedErroRoll));
	
	Para_Info.Roll_Erro.Erro=Roll_Erro;
	pid_calculate(&Para_Info.Roll_Erro,&AttitudeOutertPre,&Para_Info.Roll,&pidRoll);	
	
	/* Yaw 不需要角度控制 */
	
}

/*
 * @brief:1、3号电机在同一条对角线上逆时针旋转；2、4号电机在另一对角线上顺时针旋转；
 * 陀螺效应和空气动力扭矩效应被抵消
 */
void Calculate_output()
{

	if (FlightControl.droneMode == Drone_Mode_4Axis)
	{
		//四轴飞行模式下，PWM输出 = pitch、roll、yaw的角速度pid输出(水平) + z轴速度pid输出(高度) + 基础PWM输出
		Throttle_Info.M1 = -pidPitchRate.value - pidRollRate.value + pidHeightRate.value - pidYawRate.value + throttleBasic;

		Throttle_Info.M2 = +pidPitchRate.value - pidRollRate.value + pidHeightRate.value + pidYawRate.value + throttleBasic;

		Throttle_Info.M3 = +pidPitchRate.value + pidRollRate.value + pidHeightRate.value - pidYawRate.value + throttleBasic;

		Throttle_Info.M4 = -pidPitchRate.value + pidRollRate.value + pidHeightRate.value + pidYawRate.value + throttleBasic;
	}
	else if (FlightControl.droneMode == Drone_Mode_Pitch || FlightControl.droneMode == Drone_Mode_RatePitch)
	{
		//pitch轴 或 roll轴 角速度环调试实验
		Throttle_Info.M1 = -pidPitchRate.value + throttleBasic;
		Throttle_Info.M2 = +pidPitchRate.value + throttleBasic;
		Throttle_Info.M3 = +pidPitchRate.value + throttleBasic;
		Throttle_Info.M4 = -pidPitchRate.value + throttleBasic;
	}
	else if (FlightControl.droneMode == Drone_Mode_Roll || FlightControl.droneMode == Drone_Mode_RateRoll)
	{
		//pitch轴 或 roll轴 角度环调试实验
		Throttle_Info.M1 = -pidRollRate.value + throttleBasic;
		Throttle_Info.M2 = -pidRollRate.value + throttleBasic;
		Throttle_Info.M3 = +pidRollRate.value + throttleBasic;
		Throttle_Info.M4 = +pidRollRate.value + throttleBasic;
	}
	
	//PWM限制幅度
	if (Throttle_Info.M1 > 900)
		Throttle_Info.M1 = 900;
	if (Throttle_Info.M2 > 900)
		Throttle_Info.M2 = 900;
	if (Throttle_Info.M3 > 900)
		Throttle_Info.M3 = 900;
	if (Throttle_Info.M4 > 900)
		Throttle_Info.M4 = 900;

	if (Throttle_Info.M1 < 50)
		Throttle_Info.M1 = 50;
	if (Throttle_Info.M2 < 50)
		Throttle_Info.M2 = 50;
	if (Throttle_Info.M3 < 50)
		Throttle_Info.M3 = 50;
	if (Throttle_Info.M4 < 50)
		Throttle_Info.M4 = 50;

	PWM_OUT(Throttle_Info.M1, Throttle_Info.M2, Throttle_Info.M3, Throttle_Info.M4);
}
/*
* 根据不同实验选择不同输出通道
* Motor1-4：电机1-4的PWM输出占空比，范围为1000 - 1900
* FlightMode：飞行模式，也就是实验选择
*/
void PWM_OUT(unsigned int Motor1,
			 unsigned int Motor2,
			 unsigned int Motor3,
			 unsigned int Motor4)
{
	Motor1 += 1000;
	Motor2 += 1000;
	Motor3 += 1000;
	Motor4 += 1000;

	//设置PWM比较寄存器
	if (RT_Info.lowPowerFlag == 1)
	{
		TIM2->CCR1 = 1000;
		TIM2->CCR2 = 1000;
		TIM2->CCR3 = 1000;
		TIM2->CCR4 = 1000;
	}
	else
	{
		TIM2->CCR1 = Motor1;
		TIM2->CCR2 = Motor2;
		TIM2->CCR3 = Motor3;
		TIM2->CCR4 = Motor4;
	}
}
